﻿using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.Core.DsspHttpUtilities;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.ComponentModel;
using System.Net;
using System.Xml.Serialization;
using W3C.Soap;

using vex = VEXRobotics.Proxy;

using dssphttp = Microsoft.Dss.Core.DsspHttp;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using encoder = Microsoft.Robotics.Services.Encoder.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using motor = Microsoft.Robotics.Services.Motor.Proxy;


namespace VEXExtras.VEXRoverDrive
{

    /// <summary>
    /// VEX 6 Wheel Drive Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract(drive.Contract.Identifier)]
    [Description("Provides access to the VEX 6 Wheel Drive.")]
    [DisplayName("VEX 6 Wheel Drive")]
    [DssServiceDescription("http://msdn.microsoft.com/library/bb870567.aspx")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXRoverDrive : DsspServiceBase
    {
        #region Private State
        /// <summary>
        /// VEXRoverDrive.VEXRoverDrive State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + "/VEX.RoverDrive.config.xml")]
        private VEXRoverDriveState _state = new VEXRoverDriveState();

        private bool[] _targetEncoderPending = new bool[2];
        private const int LEFT = 0;
        private const int RIGHT = 1;
        private drive.DriveRequestOperation _internalPendingDriveOperation;

        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();
        #endregion


        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/vex/roverdrive", AllowMultipleInstances = true)]
        private DriveOperations _internalMainPort = new DriveOperations();
        private DriveOperations _reliableMainPort = new DriveOperations();


        [EmbeddedResource("VEXExtras.VEXRoverDrive.xslt")]
        string _transform = string.Empty;

        #region Partner Ports

        /// <summary>
        /// Partner with the VEX controller
        /// </summary>
        [Partner("VEX",
            Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();
        private vex.VEXOperations _vexNotificationPort = new vex.VEXOperations();

        /// <summary>
        /// Subscription manager partner for the "native" notifications
        /// </summary>
        [Partner(Partners.SubscriptionManagerString, Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Subscription manager partner for Generic notifications
        /// </summary>
        /// <remarks>All notifications through this submgr should be for pxdrive operations</remarks>
        [Partner(Partners.SubscriptionManagerString + "/genericdrive", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        submgr.SubscriptionManagerPort _genericSubMgrPort = new submgr.SubscriptionManagerPort();

        #endregion

        #region Alternate Contract Ports

        /// <summary>
        /// Generic Drive Operations Port
        /// </summary>
        [AlternateServicePort("/generic", AlternateContract = drive.Contract.Identifier)]
        private drive.DriveOperations _drivePort = new drive.DriveOperations();

        #endregion

        #region Internal CCR Ports
        // Used for driving the motors. Always execute the newest pending drive request.
        private Port<DriveDistance> _internalDrivePowerPort = new Port<DriveDistance>();
        private Port<bool>[] _targetEncoderReachedPort = new Port<bool>[2];

        #endregion


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public VEXRoverDrive(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            base.Start();

            // Add Encoder Notifications to the main interleave
            base.MainPortInterleave.CombineWith(new Interleave(
                new ExclusiveReceiverGroup(
                    Arbiter.ReceiveWithIterator<vex.Update>(true, _vexNotificationPort, NotificationHandler)
                ),
                new ConcurrentReceiverGroup()));

            // Wait one time for an InternalDrivePower command
            Activate(Arbiter.ReceiveWithIterator<DriveDistance>(false, _internalDrivePowerPort, InternalDrivePowerHandler));

            // Set up the reliable port using DSS forwarder to ensure exception and timeout conversion to fault.
            _reliableMainPort = ServiceForwarder<DriveOperations>(this.ServiceInfo.Service);

            //needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);
        }

        /// <summary>
        /// Handle periodic sensor readings from the pxbrick
        /// </summary>
        /// <param name="update"></param>
        private IEnumerator<ITask> NotificationHandler(vex.Update update)
        {
            VEXRoverDriveState currentState = (VEXRoverDriveState)_state.Clone();

            {
                int leftReversePolaritySign = (currentState.leftFront.ReversePolarity) ? -1 : 1;
                int rightReversePolaritySign = (currentState.rightFront.ReversePolarity) ? -1 : 1;

                double direction, remaining;

                _state.TimeStamp = DateTime.Now;

                long leftEncoderCount = update.Body.InterruptSensors.Sensors[(int)_state.leftEncoder - 1].Value;
                if (currentState.leftFront.ReversePolarity)
                {
                    leftEncoderCount *= -1;
                }
                if (currentState.runtimeStatistics.LeftEncoderCurrent != leftEncoderCount - currentState.runtimeStatistics.LeftTicksSinceReset)
                {
                    currentState.runtimeStatistics.LeftEncoderCurrent = leftEncoderCount - currentState.runtimeStatistics.LeftTicksSinceReset;
                    _state.runtimeStatistics.LeftEncoderCurrent = currentState.runtimeStatistics.LeftEncoderCurrent;
                    _state.runtimeStatistics.TimeStamp = currentState.runtimeStatistics.TimeStamp = DateTime.Now;

                    direction = Math.Sign(currentState.runtimeStatistics.LeftPowerTarget);
                    if (0 == direction)
                        direction = 1;
                    
                    // negative remaining means we have passed the target.
                    remaining = currentState.runtimeStatistics.LeftEncoderTarget - leftEncoderCount;
                    remaining *= _state.leftFront.ReversePolarity ? -1 : 1;
                    if (currentState.runtimeStatistics.LeftEncoderTarget != 0)
                    {
                        if (remaining <= 5 && _targetEncoderPending[LEFT])
                        {
                            _state.runtimeStatistics.LeftPowerCurrent = 0.0;
                            _state.runtimeStatistics.LeftPowerTarget = 0.0;

                            _targetEncoderReachedPort[LEFT].Post(true);
                            _targetEncoderPending[LEFT] = false;

                            if (_internalPendingDriveOperation == drive.DriveRequestOperation.RotateDegrees)
                            {
                                _state.runtimeStatistics.RightPowerCurrent = 0.0;
                                _state.runtimeStatistics.RightPowerTarget = 0.0;

                                _targetEncoderReachedPort[RIGHT].Post(true);
                                _targetEncoderPending[RIGHT] = false;
                            }
                        }
                    }
                }

                long rightEncoderCount = update.Body.InterruptSensors.Sensors[(int)_state.rightEncoder - 1].Value;
                if (currentState.rightFront.ReversePolarity)
                {
                    rightEncoderCount *= -1;
                }
                if (currentState.runtimeStatistics.RightEncoderCurrent != rightEncoderCount - currentState.runtimeStatistics.RightTicksSinceReset)
                {
                    currentState.runtimeStatistics.RightEncoderCurrent = rightEncoderCount;
                    _state.runtimeStatistics.RightEncoderCurrent = currentState.runtimeStatistics.RightEncoderCurrent - currentState.runtimeStatistics.RightTicksSinceReset;

                    direction = Math.Sign(currentState.runtimeStatistics.RightPowerTarget);
                    if (0 == direction)
                        direction = 1;

                    // negative remaining means we have passed the target.
                    remaining = currentState.runtimeStatistics.RightEncoderTarget - rightEncoderCount;
                    remaining *= _state.rightFront.ReversePolarity ? -1 : 1;
                    if (currentState.runtimeStatistics.RightEncoderTarget != 0)
                    {
                        if (remaining <= 5 && _targetEncoderPending[RIGHT])
                        {
                            _state.runtimeStatistics.RightPowerCurrent = 0.0;
                            _state.runtimeStatistics.RightPowerTarget = 0.0;

                            _targetEncoderReachedPort[RIGHT].Post(true);
                            _targetEncoderPending[RIGHT] = false;

                            if (_internalPendingDriveOperation == drive.DriveRequestOperation.RotateDegrees)
                            {
                                _state.runtimeStatistics.LeftPowerCurrent = 0.0;
                                _state.runtimeStatistics.LeftPowerTarget = 0.0;

                                _targetEncoderReachedPort[LEFT].Post(true);
                                _targetEncoderPending[LEFT] = false;
                            }

                        }
                    }
                }
            }
            yield break;
        }

        /// <summary>
        /// Setup a motor power definition
        /// </summary>
        /// <param name="motorPort">Motor port to setup</param>
        /// <param name="power">Power level to set</param>
        /// <returns></returns>
        private vex.SetMotor SetupMotor(vex.MotorPort motorPort, int power)
        {
            vex.SetMotor motor = new vex.SetMotor();
            motor.Motor = motorPort;
            motor.Power = power;
            return motor;
        }

        /// <summary>
        /// Process the most recent Drive Power command
        /// When complete, self activate for the next internal command
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalDrivePowerHandler(DriveDistance driveDistance)
        {
            try
            {
                #region Check for a backlog of drive commands

                // Take a snapshot of the number of pending commands at the time
                // we entered this routine.
                // This will prevent a livelock which can occur if we try to
                // process the queue until it is empty, but the inbound queue
                // is growing at the same rate as we are pulling off the queue.
                int pendingCommands = _internalDrivePowerPort.ItemCount;

                // If newer commands have been issued,
                // respond success to the older command and
                // move to the newer one.
                DriveDistance newerUpdate;
                while (pendingCommands > 0)
                {
                    if (_internalDrivePowerPort.Test(out newerUpdate))
                    {
                        driveDistance = newerUpdate;
                    }
                    pendingCommands--;
                }
                #endregion

                // Get a snapshot of our state. 
                VEXRoverDriveState currentState = (VEXRoverDriveState)_state.Clone();

                #region Cancel any prior encoder target

                // If a prior encoder target was active, 
                // signal that it is cancelled by a new Motor command.
                if (_targetEncoderPending[LEFT] || _targetEncoderPending[RIGHT])
                {
                    lock (_targetEncoderPending)
                    {
                        if (_targetEncoderPending[LEFT])
                        {
                            _targetEncoderReachedPort[LEFT].Post(false);
                            _targetEncoderPending[LEFT] = false;
                        }
                        if (_targetEncoderPending[RIGHT])
                        {
                            _targetEncoderReachedPort[RIGHT].Post(false);
                            _targetEncoderPending[RIGHT] = false;
                        }

                        _state.runtimeStatistics.LeftEncoderTarget = 0;
                        _state.runtimeStatistics.RightEncoderTarget = 0;
                    }
                }
                #endregion

                // Calculate the motor power and target degrees for both motors.
                int leftMotorPower, rightMotorPower;
                long leftTargetEncoderTics, rightTargetEncoderTics;
                CalculatePowerAndTargetDegrees(driveDistance.Body, currentState,
                    out leftMotorPower,
                    out rightMotorPower,
                    out leftTargetEncoderTics,
                    out rightTargetEncoderTics);

                _state.runtimeStatistics.LeftPowerCurrent = ((double)leftMotorPower - 127.0) / 127.0; ;
                _state.runtimeStatistics.LeftPowerTarget = driveDistance.Body.LeftPower;
                _state.runtimeStatistics.RightPowerCurrent = ((double)rightMotorPower - 127.0) / 127.0;
                _state.runtimeStatistics.RightPowerTarget = driveDistance.Body.RightPower;

                if (leftTargetEncoderTics != 0)
                {
                    // Calcuate a new target encoder which is based on the current encoder position.
                    leftTargetEncoderTics = (leftTargetEncoderTics * (_state.leftFront.ReversePolarity ? -1 : 1)) + currentState.runtimeStatistics.LeftEncoderCurrent;

                    // A target of zero diables the PD control.  Make sure our result isn't 0.
                    if (leftTargetEncoderTics == 0)
                        leftTargetEncoderTics++;

                    _targetEncoderPending[LEFT] = true;
                }
                _state.runtimeStatistics.LeftEncoderTarget = leftTargetEncoderTics;

                if (rightTargetEncoderTics != 0)
                {
                    // Calcuate a new target encoder which is based on the current encoder position.
                    rightTargetEncoderTics = (rightTargetEncoderTics * (_state.rightFront.ReversePolarity ? -1 : 1)) + currentState.runtimeStatistics.RightEncoderCurrent;

                    // A target of zero diables the PD control.  Make sure our result isn't 0.
                    if (rightTargetEncoderTics == 0)
                        rightTargetEncoderTics++;

                    _targetEncoderPending[RIGHT] = true;
                }
                _state.runtimeStatistics.RightEncoderTarget = rightTargetEncoderTics;

                _state.TimeStamp = DateTime.Now;

                // Setup to send the motors a command
                vex.SetMotorList motorCmd = new vex.SetMotorList();
                motorCmd.Motors = new List<vex.SetMotor>();

                // Send the left motor command to the controller
                motorCmd.Motors.Add(SetupMotor(currentState.leftFront.MotorPort, leftMotorPower));
                motorCmd.Motors.Add(SetupMotor(currentState.leftMiddle.MotorPort, leftMotorPower));
                motorCmd.Motors.Add(SetupMotor(currentState.leftRear.MotorPort, leftMotorPower));

                // Send the right motor comand to the brick
                motorCmd.Motors.Add(SetupMotor(currentState.rightFront.MotorPort, rightMotorPower));
                motorCmd.Motors.Add(SetupMotor(currentState.rightMiddle.MotorPort, rightMotorPower));
                motorCmd.Motors.Add(SetupMotor(currentState.rightRear.MotorPort, rightMotorPower));

                Fault fault = null;
                Activate(Arbiter.Choice(_vexPort.SetMotorRequest(motorCmd),
                    delegate(DefaultUpdateResponseType response)
                    {
                    },
                    delegate(Fault aFault)
                    {
                        fault = aFault;
                    }
                ));

                if (leftTargetEncoderTics != 0 && rightTargetEncoderTics != 0)
                {
                    // Wait for the target encoders to either be cancelled or reach its target.
                    Activate(Arbiter.MultiplePortReceive<bool>(false, _targetEncoderReachedPort,
                        delegate(bool[] finished)
                        {
                            HandleDriveResponse(driveDistance, finished[LEFT] && finished[RIGHT], null);
                        }));
                }
                else if (leftTargetEncoderTics != 0 || rightTargetEncoderTics != 0)
                {
                    int ix = (leftTargetEncoderTics != 0) ? LEFT : RIGHT;

                    // Wait for the target encoder to either be cancelled or reach its target.
                    Activate(Arbiter.Receive<bool>(false, _targetEncoderReachedPort[ix],
                        delegate(bool finished)
                        {
                            HandleDriveResponse(driveDistance, finished, null);
                        }));
                }
                else
                {
                    bool success = (fault == null);
                    HandleDriveResponse(driveDistance, success, fault);
                }
            }
            finally
            {
                // Wait one time for the next InternalDrivePower command
                Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler));
            }
            yield break;
        }

        #region Main Port Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> MainGetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> MainHttpGetHandler(HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(HttpStatusCode.OK, _state, _transform));
            yield break;
        }

        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);
            yield return (Choice)readForm.ResultPort;
            Exception exception = (Exception)readForm.ResultPort;
            if (exception != null)
            {
                LogError(exception);
                httpPost.ResponsePort.Post(new HttpResponseType(
                    HttpStatusCode.OK,
                    Fault.FromException(exception)));

                yield break;
            }

            NameValueCollection parameters = readForm.ResultPort;
            if (parameters["EnableDrive"] != null)
            {
                drive.EnableDrive enableDrive = new drive.EnableDrive();
                enableDrive.Body = new drive.EnableDriveRequest();
                enableDrive.Body.Enable = true;
                _drivePort.Post(enableDrive);
                yield return Arbiter.Choice(enableDrive.ResponsePort,
                    delegate(DefaultUpdateResponseType response)
                    {
                        HttpPostSuccess(httpPost);
                    },
                    delegate(Fault fault)
                    {
                        HttpPostFailure(httpPost, fault.ToString());
                    });
            }
            else if (parameters["DisableDrive"] != null)
            {
                drive.EnableDrive enableDrive = new drive.EnableDrive();
                enableDrive.Body = new drive.EnableDriveRequest();
                enableDrive.Body.Enable = false;
                _drivePort.Post(enableDrive);
                yield return Arbiter.Choice(enableDrive.ResponsePort,
                    delegate(DefaultUpdateResponseType response)
                    {
                        HttpPostSuccess(httpPost);
                    },
                    delegate(Fault fault)
                    {
                        HttpPostFailure(httpPost, fault.ToString());
                    });
            }
            else if (parameters["AllStop"] != null)
            {
                drive.AllStop allStop = allStop = new drive.AllStop();
                allStop.Body = new drive.AllStopRequest();
                _drivePort.Post(allStop);
                yield return Arbiter.Choice(allStop.ResponsePort,
                    delegate(DefaultUpdateResponseType response)
                    {
                        HttpPostSuccess(httpPost);
                    },
                    delegate(Fault fault)
                    {
                        HttpPostFailure(httpPost, fault.ToString());
                    });
            }
            else if (parameters["CustomPower"] != null)
            {
                double leftpower = 0;
                double.TryParse(parameters["LeftPower"], out leftpower);
                double rightpower = 0;
                double.TryParse(parameters["RightPower"], out rightpower);

                drive.SetDrivePower drivePower = new drive.SetDrivePower();
                drivePower.Body = new drive.SetDrivePowerRequest();
                drivePower.Body.LeftWheelPower = leftpower;
                drivePower.Body.RightWheelPower = rightpower;
                _drivePort.Post(drivePower);
                yield return Arbiter.Choice(drivePower.ResponsePort,
                    delegate(DefaultUpdateResponseType response)
                    {
                        HttpPostSuccess(httpPost);
                    },
                    delegate(Fault fault)
                    {
                        HttpPostFailure(httpPost, fault.ToString());
                    });
            }

            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        private static void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.ExpectationFailed, failureReason);
            httpPost.ResponsePort.Post(rsp);
        }
        
        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="allStop"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> MainAllStopHandler(drive.AllStop allStop)
        {
            DriveDistance driveReq = new DriveDistance(new SetDriveRequest());
            driveReq.Body.LeftPower = 0.0;
            driveReq.Body.RightPower = 0.0;
            driveReq.Body.LeftStopAtRotationDegrees = 0;
            driveReq.Body.RightStopAtRotationDegrees = 0;
            driveReq.ResponsePort = allStop.ResponsePort;
            driveReq.Body.isGenericOperation = true;
            driveReq.Body.DriveRequestOperation = drive.DriveRequestOperation.AllStop;
            _internalDrivePowerPort.Post(driveReq);
            yield break;
        }

        /// <summary>
        /// ResetEncoders handler
        /// </summary>
        /// <param name="resetEncoders">ResetEncoders message</param>
        /// <returns>A CCR task enumerator</returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_drivePort")]
        public IEnumerator<ITask> ResetEncodersHandler(drive.ResetEncoders resetEncoders)
        {
            this._state.runtimeStatistics.LeftEncoderTarget -= this._state.runtimeStatistics.LeftEncoderCurrent + this._state.runtimeStatistics.LeftTicksSinceReset;
            this._state.runtimeStatistics.RightEncoderTarget -= this._state.runtimeStatistics.RightEncoderCurrent + this._state.runtimeStatistics.RightTicksSinceReset;
            this._state.runtimeStatistics.LeftTicksSinceReset = this._state.runtimeStatistics.LeftEncoderCurrent;
            this._state.runtimeStatistics.RightTicksSinceReset = this._state.runtimeStatistics.RightEncoderCurrent;
            this._state.runtimeStatistics.LeftEncoderCurrent = 0;
            this._state.runtimeStatistics.RightEncoderCurrent = 0;

            this.SendNotification<drive.ResetEncoders>(this._subMgrPort, resetEncoders.Body);
            resetEncoders.ResponsePort.Post(new DefaultUpdateResponseType());

            yield break;
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> MainDriveDistanceHandler(DriveDistance driveDistance)
        {
            _internalDrivePowerPort.Post(driveDistance);
            yield break;
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> MainSubscribeHandler(Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }

        #endregion

        #region Generic Drive Handlers

        /// <summary>
        /// Generic Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericGetHandler(drive.Get get)
        {
            drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
            get.ResponsePort.Post(_state.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Generic HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            HttpResponseType rsp = new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            get.ResponsePort.Post(rsp);
            yield break;
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericSubscribeHandler(drive.Subscribe subscribe)
        {
            base.SubscribeHelper(_genericSubMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_genericSubMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }

        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="updateGenericDrive"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericUpdateHandler(drive.Update updateGenericDrive)
        {
            _state.FromGenericState(updateGenericDrive.Body);
            updateGenericDrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="enableDrive"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericEnableDriveHandler(drive.EnableDrive enableDrive)
        {
            _state.IsEnabled = enableDrive.Body.Enable;
            _state.TimeStamp = DateTime.Now;

            SendNotification<drive.EnableDrive>(_genericSubMgrPort, enableDrive);
            enableDrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;

        }

        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="drivePower"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericSetDrivePowerHandler(drive.SetDrivePower drivePower)
        {
            // set back response immediately or fault if drive is not enabled.
            if (!ValidateDriveEnabledAndRespondHelper(drivePower.ResponsePort))
                yield break;

            DriveDistance drive = new DriveDistance(new SetDriveRequest());
            drive.Body.LeftPower = drivePower.Body.LeftWheelPower;
            drive.Body.RightPower = drivePower.Body.RightWheelPower;
            drive.Body.LeftStopAtRotationDegrees = 0;
            drive.Body.RightStopAtRotationDegrees = 0;
            drive.Body.isGenericOperation = true;
            drive.ResponsePort = drivePower.ResponsePort;
            _internalDrivePowerPort.Post(drive);
            yield break;
        }

        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="driveSpeed"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericSetDriveSpeedHandler(drive.SetDriveSpeed driveSpeed)
        {
            // set back response immediately or fault if drive is not enabled.
            if (!ValidateDriveEnabledAndRespondHelper(driveSpeed.ResponsePort))
                yield break;

            DriveDistance drive = new DriveDistance(new SetDriveRequest());
            drive.Body.LeftPower = driveSpeed.Body.LeftWheelSpeed;
            drive.Body.RightPower = driveSpeed.Body.RightWheelSpeed;
            drive.Body.LeftStopAtRotationDegrees = 0;
            drive.Body.RightStopAtRotationDegrees = 0;
            drive.Body.isGenericOperation = true;

            drive.ResponsePort = driveSpeed.ResponsePort;
            _internalDrivePowerPort.Post(drive);
            yield break;
        }

        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="rotateDegrees"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
        {
            if (_state.DistanceBetweenWheels <= 0)
            {
                rotateDegrees.ResponsePort.Post(Fault.FromException(new ArgumentOutOfRangeException("DistanceBetweenWheels must be specified in the Drive Configuration.")));
                yield break;
            }

            // set back response immediately or fault if drive is not enabled.
            if (!ValidateDriveEnabledAndRespondHelper(rotateDegrees.ResponsePort))
                yield break;

            // distance = circumference / 360 * degreesToTurn
            double distance = Math.PI * _state.DistanceBetweenWheels * rotateDegrees.Body.Degrees / 360.0;

            // axleRotationDegrees = distance (meters) / wheelCircumference (pi * diameter) * 360
            long axleRotationDegrees = (long)Math.Round(Math.Abs(distance) / (Math.PI * _state.leftFront.WheelDiameter) * 360.0);

            LogInfo("RotateDegrees: Wheel Distance: " + distance + "  Axle Rotation Degrees: " + axleRotationDegrees);

            double leftDirection = 1.0, rightDirection = 1.0;
            if (rotateDegrees.Body.Degrees > 0)
                leftDirection = -1.0;
            else
                rightDirection = -1.0;

            DriveDistance driveReq = new DriveDistance(new SetDriveRequest());
            driveReq.Body.LeftStopAtRotationDegrees = axleRotationDegrees * (_state.leftFront.ReversePolarity ? -1 : 1);
            driveReq.Body.RightStopAtRotationDegrees = axleRotationDegrees * (_state.rightFront.ReversePolarity ? -1 : 1);
            driveReq.Body.isGenericOperation = true;
            driveReq.Body.DriveRequestOperation = drive.DriveRequestOperation.RotateDegrees;
            _internalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees;
            bool synchronized = false;

            if (synchronized)
            {
                driveReq.Body.LeftPower = Math.Abs(rotateDegrees.Body.Power);
                driveReq.Body.RightPower = Math.Abs(rotateDegrees.Body.Power);
            }
            else
            {
                driveReq.Body.LeftPower = rotateDegrees.Body.Power * leftDirection;
                driveReq.Body.RightPower = rotateDegrees.Body.Power * rightDirection;
            }
            driveReq.ResponsePort = rotateDegrees.ResponsePort;

            // notify subscribers of rotate degrees start
            rotateDegrees.Body.RotateDegreesStage = drive.DriveStage.Started;
            drive.RotateDegrees rotateDegreesUpdate = new drive.RotateDegrees(rotateDegrees.Body);
            SendNotification<drive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);

            _internalDrivePowerPort.Post(driveReq);
            yield break;
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericDriveDistanceHandler(drive.DriveDistance driveDistance)
        {
            double distance = driveDistance.Body.Distance;

            // set back response immediately or fault if drive is not enabled.
            if (!ValidateDriveEnabledAndRespondHelper(driveDistance.ResponsePort))
                yield break;

            // rotations = distance (meters) / circumference (pi * diameter) 
            // degrees = rotations * 90
            double stopLeftWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.leftFront.WheelDiameter) * 360.0);
            double stopRightWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.rightFront.WheelDiameter) * 360.0);

            DriveDistance driveReq = new DriveDistance(new SetDriveRequest());
            driveReq.Body.LeftPower = driveDistance.Body.Power;
            driveReq.Body.RightPower = driveDistance.Body.Power;
            driveReq.Body.LeftStopAtRotationDegrees = (long)stopLeftWheelAtDegrees;
            driveReq.Body.RightStopAtRotationDegrees = (long)stopRightWheelAtDegrees;
            driveReq.Body.isGenericOperation = true;
            driveReq.Body.DriveRequestOperation = drive.DriveRequestOperation.DriveDistance;
            _internalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance;
            driveReq.ResponsePort = driveDistance.ResponsePort;

            // notify subscribers of drive distance start
            driveDistance.Body.DriveDistanceStage = drive.DriveStage.Started;
            drive.DriveDistance driveDistanceUpdate = new drive.DriveDistance(driveDistance.Body);
            SendNotification<drive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);

            _internalDrivePowerPort.Post(driveReq);
            yield break;
        }

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="allStop"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_drivePort")]
        public virtual IEnumerator<ITask> GenericAllStopHandler(drive.AllStop allStop)
        {
            DriveDistance driveReq = new DriveDistance(new SetDriveRequest());
            driveReq.Body.LeftPower = 0.0;
            driveReq.Body.RightPower = 0.0;
            driveReq.Body.LeftStopAtRotationDegrees = 0;
            driveReq.Body.RightStopAtRotationDegrees = 0;
            driveReq.ResponsePort = allStop.ResponsePort;
            driveReq.Body.DriveRequestOperation = drive.DriveRequestOperation.AllStop;
            driveReq.Body.isGenericOperation = true;
            _internalDrivePowerPort.Post(driveReq);
            allStop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        #endregion

        #region Internal Methods

        /// <summary>
        /// Setup a wheel definition
        /// </summary>
        /// <param name="name">Name of the wheel</param>
        /// <param name="hwId">Motor port used</param>
        /// <param name="reverse"></param>
        /// <returns></returns>
        WheelConfiguration SetupWheel(string name, vex.MotorPort motorPort, bool reverse)
        {
            WheelConfiguration aWheel;

            aWheel = new WheelConfiguration();
            aWheel.MotorPort = motorPort;
            aWheel.ReversePolarity = reverse;
            aWheel.WheelDiameter = 0.07;
            return aWheel;
        }

        /// <summary>
        /// Initialize and validate startup DriveState
        /// </summary>
        void InitializeState()
        {
            _targetEncoderReachedPort[LEFT] = new Port<bool>();
            _targetEncoderReachedPort[RIGHT] = new Port<bool>();
            _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;

            // if (null == _state)
            {
                _state = new VEXRoverDriveState();

                //initialize motor platform specific to VEX
                _state.DistanceBetweenWheels = 0.34;
                _state.TimeStamp = DateTime.Now;
                _state.IsEnabled = true;

                _state.leftFront = SetupWheel("Left Front", vex.MotorPort.MotorPort_1, false);
                _state.leftMiddle = SetupWheel("Left Middle", vex.MotorPort.MotorPort_2, false);
                _state.leftRear = SetupWheel("Left Rear", vex.MotorPort.MotorPort_3, false);

                _state.rightFront = SetupWheel("Right Front", vex.MotorPort.MotorPort_4, true);
                _state.rightMiddle = SetupWheel("Right Middle", vex.MotorPort.MotorPort_5, true);
                _state.rightRear = SetupWheel("Right Rear", vex.MotorPort.MotorPort_6, true);

                _state.leftEncoder = vex.InterruptPort.InterruptPort_1;
                _state.rightEncoder = vex.InterruptPort.InterruptPort_2;
            }

            // Always initialize the runtime statistics when we start.
            _state.runtimeStatistics = new RuntimeStatistics();

            // Make sure the motors on the controller is properly configured
            VEXRobotics.Proxy.MotorConfigData motor1Data = new VEXRobotics.Proxy.MotorConfigData();
            motor1Data.Motor = (vex.MotorPort)_state.leftFront.MotorPort;
            motor1Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor1Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor1Data);
            _vexPort.Post(motor1Config);
            VEXRobotics.Proxy.MotorConfigData motor2Data = new VEXRobotics.Proxy.MotorConfigData();
            motor2Data.Motor = (vex.MotorPort)_state.leftMiddle.MotorPort;
            motor2Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor2Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor2Data);
            _vexPort.Post(motor2Config);
            VEXRobotics.Proxy.MotorConfigData motor3Data = new VEXRobotics.Proxy.MotorConfigData();
            motor3Data.Motor = (vex.MotorPort)_state.leftRear.MotorPort;
            motor3Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor3Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor3Data);
            _vexPort.Post(motor3Config);
            VEXRobotics.Proxy.MotorConfigData motor4Data = new VEXRobotics.Proxy.MotorConfigData();
            motor4Data.Motor = (vex.MotorPort)_state.rightFront.MotorPort;
            motor4Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor4Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor4Data);
            _vexPort.Post(motor4Config);
            VEXRobotics.Proxy.MotorConfigData motor5Data = new VEXRobotics.Proxy.MotorConfigData();
            motor5Data.Motor = (vex.MotorPort)_state.rightMiddle.MotorPort;
            motor5Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor5Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor5Data);
            _vexPort.Post(motor5Config);
            VEXRobotics.Proxy.MotorConfigData motor6Data = new VEXRobotics.Proxy.MotorConfigData();
            motor6Data.Motor = (vex.MotorPort)_state.rightRear.MotorPort;
            motor6Data.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure motor6Config = new VEXRobotics.Proxy.SelectiveMotorConfigure(motor6Data);
            _vexPort.Post(motor6Config);

            // Make sure the encoders on the controller is properly configured
            VEXRobotics.Proxy.InterruptPortConfigData intPort1Data = new VEXRobotics.Proxy.InterruptPortConfigData();
            intPort1Data.Interrupt = _state.leftEncoder;
            intPort1Data.Type = VEXRobotics.Proxy.SensorDefinition.SensorType.Encoder;
            VEXRobotics.Proxy.SelectiveInterruptConfigure int1Config = new VEXRobotics.Proxy.SelectiveInterruptConfigure(intPort1Data);
            _vexPort.Post(int1Config);
            VEXRobotics.Proxy.InterruptPortConfigData intPort2Data = new VEXRobotics.Proxy.InterruptPortConfigData();
            intPort2Data.Interrupt = _state.rightEncoder;
            intPort2Data.Type = VEXRobotics.Proxy.SensorDefinition.SensorType.Encoder;
            VEXRobotics.Proxy.SelectiveInterruptConfigure int2Config = new VEXRobotics.Proxy.SelectiveInterruptConfigure(intPort2Data);
            _vexPort.Post(int2Config);

            _vexPort.Subscribe(_vexNotificationPort, typeof(vex.Update));
            
            SaveState(_state);
        }

        /// <summary>
        /// Post a success or fault to complete a DriveDistance request.
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <param name="success"></param>
        /// <param name="fault"></param>
        private void HandleDriveResponse(DriveDistance driveDistance, bool success, Fault fault)
        {
            if (success)
            {
                if (driveDistance.Body.isGenericOperation == true)
                {
                    //notify subscribers of generic drive distance -- completed
                    HandleDriveResponseForGenericOperationsNotifications(driveDistance, success, fault);
                }
                else
                {
                    //notify subscribers of specific drive distance -- completed
                }
            }
            else
            {
                string msg;
                if (fault == null)
                {
                    msg = "The Drive operation was canceled due to a newer drive command.";
                    fault = Fault.FromException(new OperationCanceledException(msg));
                }
                else
                {
                    msg = fault.ToString();
                }
                driveDistance.ResponsePort.Post(fault);
                LogVerbose(LogGroups.Console, msg);
            }
        }

        /// <summary>
        /// Notified subscribers a success or fault to completed Drive request.
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <param name="success"></param>
        /// <param name="fault"></param>
        public void HandleDriveResponseForGenericOperationsNotifications(DriveDistance driveDistance, bool success, Fault fault)
        {
            if (fault == null)
            {
                if (driveDistance.Body.isGenericOperation == true)
                {
                    //notify subscribers of generic drive distance -- complete
                    switch (driveDistance.Body.DriveRequestOperation)
                    {
                        case drive.DriveRequestOperation.DriveDistance:
                            drive.DriveDistanceRequest driveDistanceRequest = new drive.DriveDistanceRequest();
                            driveDistanceRequest.DriveDistanceStage = drive.DriveStage.Completed;

                            drive.DriveDistance driveDistanceUpdate = new drive.DriveDistance(driveDistanceRequest);
                            SendNotification<drive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);
                            break;

                        case drive.DriveRequestOperation.RotateDegrees:
                            drive.RotateDegreesRequest rotateDegreesRequest = new drive.RotateDegreesRequest();
                            rotateDegreesRequest.RotateDegreesStage = drive.DriveStage.Completed;

                            drive.RotateDegrees rotateDegreesUpdate = new drive.RotateDegrees(rotateDegreesRequest);
                            SendNotification<drive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);
                            break;
                    }
                }
                else
                {
                    // Operation canceled.
                    driveDistance.Body.DriveDistanceStage = drive.DriveStage.Canceled;
                    SendNotification<drive.DriveDistance>(_genericSubMgrPort, driveDistance.Body);
                }
                _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
            }
        }

        /// <summary>
        /// Calculate the maximum allowable target motor power (+-100)
        /// based on the remaining degrees to travel.
        /// </summary>
        /// <param name="degreesRemaining">Negative values indicate we are past the target.</param>
        /// <param name="targetPower"></param>
        /// <param name="rpm"></param>
        /// <param name="degreesAtNextNotification"></param>
        /// <returns></returns>
        internal static int CalculateMaxMotorPower(long ticsRemaining, double targetPower, double rpm, double degreesAtNextNotification)
        {
            int motorPower = Math.Min(127, Math.Max(-127, (int)(targetPower * 127.0)));

            // Not stopping
            if (ticsRemaining == 0)
                return motorPower;

            // overshot
            if (ticsRemaining < 0)
                return 0;

            // Less than half a turn
            if (ticsRemaining <= 45)
                return Math.Min(Math.Abs(motorPower), (int)ticsRemaining / 7) * Math.Sign(targetPower);

            if (ticsRemaining <= 68)
                return Math.Min(Math.Abs(motorPower), (int)ticsRemaining / 6) * Math.Sign(targetPower);

            // Max power is 25% of the target degrees.
            if (ticsRemaining < Math.Abs(motorPower))
                return Math.Min(Math.Abs(motorPower), (int)ticsRemaining / 5) * Math.Sign(targetPower);

            if (degreesAtNextNotification != 0 && ticsRemaining < degreesAtNextNotification)
                return Math.Min(Math.Abs(motorPower / 3), (int)(10.0 * (double)motorPower / degreesAtNextNotification)) * Math.Sign(targetPower);

            return motorPower;
        }

        /// <summary>
        /// Calculate the optimal power and target encoder degrees.
        /// </summary>
        /// <param name="drive">SetDriveRequest</param>
        /// <param name="currentState">DriveState</param>
        /// <param name="leftMotorPower"></param>
        /// <param name="rightMotorPower"></param>
        /// <param name="leftTargetEncoderDegrees"></param>
        /// <param name="rightTargetEncoderDegrees"></param>
        private void CalculatePowerAndTargetDegrees(
            SetDriveRequest drive,
            VEXRoverDriveState currentState,
            out int leftMotorPower,
            out int rightMotorPower,
            out long leftTargetEncoderTics,
            out long rightTargetEncoderTics)
        {
            leftTargetEncoderTics = Math.Abs(drive.LeftStopAtRotationDegrees) / 4;
            rightTargetEncoderTics = Math.Abs(drive.RightStopAtRotationDegrees) / 4;

            leftMotorPower = CalculateMaxMotorPower(leftTargetEncoderTics, drive.LeftPower * (currentState.leftFront.ReversePolarity ? -1.0 : 1.0), 0.0, 0.0);
            leftMotorPower += 127;
            rightMotorPower = CalculateMaxMotorPower(rightTargetEncoderTics, drive.RightPower * (currentState.rightFront.ReversePolarity ? -1.0 : 1.0), 0.0, 0.0);
            rightMotorPower += 127;

            // Adjust encoder sign to match power.
            if (Math.Sign(leftMotorPower) != Math.Sign(leftTargetEncoderTics))
                leftTargetEncoderTics *= -1;
            if (Math.Sign(rightMotorPower) != Math.Sign(rightTargetEncoderTics))
                rightTargetEncoderTics *= -1;
        }

        /// <summary>
        /// Validate that the drive is enabled
        /// </summary>
        /// <param name="responsePort">Port to notify a valid request upon</param>
        /// <returns>True if the drive is enabled</returns>
        private bool ValidateDriveEnabledAndRespondHelper(PortSet<DefaultUpdateResponseType, Fault> responsePort)
        {
            Fault fault = null;

            // Acknowledge request or fault
            if (_state.IsEnabled == false)
            {
                fault = Fault.FromException(new InvalidOperationException("Attempting to process a drive operation, but the differential drive is not enabled"));
                responsePort.Post(fault);
            }
            else
            {
                // responsePort.Post(DefaultUpdateResponseType.Instance);
            }
            return _state.IsEnabled;
        }

        #endregion
    }

    /// <summary>
    /// VEXRoverDrive Contract
    /// </summary>
    public sealed class Contract
    {
        /// The Unique Contract Identifier for the VEXRoverDrive service
        [DataMember, Description("Identifies the Unique Contract Identifier for the VEXRoverDrive service.")]
        public const String Identifier = "http://schemas.vexrobotics.com/2011/11/vex/roverdrive.html";
    }

    [DataContract]
    [Description("The rover drive state.")]
    public class VEXRoverDriveState : IDssSerializable, ICloneable
    {
        double _distanceBetweenWheels;
        bool _isEnabled;
        System.DateTime _timestamp;
        RuntimeStatistics _runtimeStatistics;
        WheelConfiguration _leftFront;
        WheelConfiguration _leftMiddle;
        WheelConfiguration _leftRear;
        WheelConfiguration _rightFront;
        WheelConfiguration _rightMiddle;
        WheelConfiguration _rightRear;

        vex.InterruptPort _rightEncoder;
        vex.InterruptPort _leftEncoder;

        [DataMember]
        [Description("Left front motor.")]
        public double DistanceBetweenWheels
        {
            get { return _distanceBetweenWheels; }
            set { _distanceBetweenWheels = value; }
        }

        [DataMember]
        [Description("Left front motor.")]
        public bool IsEnabled
        {
            get { return _isEnabled; }
            set { _isEnabled = value; }
        }

        [DataMember]
        [Description("Left front motor.")]
        public System.DateTime TimeStamp
        {
            get { return _timestamp; }
            set { _timestamp = value; }
        }

        [DataMember]
        [Description("Runtime statistics.")]
        public RuntimeStatistics runtimeStatistics
        {
            get { return _runtimeStatistics; }
            set { _runtimeStatistics = value; }
        }

        [DataMember]
        [Description("Left front motor.")]
        public WheelConfiguration leftFront
        {
            get { return _leftFront; }
            set { _leftFront = value; }
        }

        [DataMember]
        [Description("Left middle motor.")]
        public WheelConfiguration leftMiddle
        {
            get { return _leftMiddle; }
            set { _leftMiddle = value; }
        }

        [DataMember]
        [Description("Left rear motor.")]
        public WheelConfiguration leftRear
        {
            get { return _leftRear; }
            set { _leftRear = value; }
        }

        [DataMember]
        [Description("Right front motor.")]
        public WheelConfiguration rightFront
        {
            get { return _rightFront; }
            set { _rightFront = value; }
        }

        [DataMember]
        [Description("Right middle motor.")]
        public WheelConfiguration rightMiddle
        {
            get { return _rightMiddle; }
            set { _rightMiddle = value; }
        }

        [DataMember]
        [Description("Right rear motor.")]
        public WheelConfiguration rightRear
        {
            get { return _rightRear; }
            set { _rightRear = value; }
        }
        
        [DataMember]
        [Description("Right side encoder")]
        public vex.InterruptPort rightEncoder
        {
            get { return _rightEncoder; }
            set { _rightEncoder = value; }
        }

        [DataMember]
        [Description("Left side encoder")]
        public vex.InterruptPort leftEncoder
        {
            get { return _leftEncoder; }
            set { _leftEncoder = value; }
        }

        public void FromGenericState(drive.DriveDifferentialTwoWheelState genericState)
        {
            this.DistanceBetweenWheels = genericState.DistanceBetweenWheels;
            this.IsEnabled = genericState.IsEnabled;
            this.TimeStamp = genericState.TimeStamp;
        }

        public drive.DriveDifferentialTwoWheelState ToGenericState(drive.DriveDifferentialTwoWheelState genericState)
        {
            genericState.DistanceBetweenWheels = DistanceBetweenWheels;
            genericState.DriveDistanceStage = drive.DriveStage.Completed;
            genericState.DriveState = drive.DriveState.Stopped;
            genericState.IsEnabled = _isEnabled;
            genericState.LeftWheel = new motor.WheeledMotorState();
            genericState.LeftWheel.EncoderState = new encoder.EncoderState((int)this.leftEncoder, 90);
            genericState.LeftWheel.EncoderState.CurrentAngle = this.runtimeStatistics.LeftEncoderCurrent % (2.0 * Math.PI);
            genericState.LeftWheel.EncoderState.CurrentReading = (int) this.runtimeStatistics.LeftEncoderCurrent;
            genericState.LeftWheel.EncoderState.TicksSinceReset = genericState.LeftWheel.EncoderState.CurrentReading;
            genericState.LeftWheel.EncoderState.TimeStamp = TimeStamp;
            genericState.LeftWheel.GearRatio = 1.0;
            genericState.LeftWheel.MotorState = new motor.MotorState("Left Front", (int) leftFront.MotorPort, 1.0, false, new Microsoft.Robotics.PhysicalModel.Proxy.Pose());
            genericState.LeftWheel.MotorState.CurrentPower = 0;
            genericState.LeftWheel.MotorState.HardwareIdentifier = (int) leftFront.MotorPort;
            /* FIX ME - RightWheel */
            genericState.RotateDegreesStage = drive.DriveStage.Completed;
            genericState.TimeStamp = DateTime.Now;
            return genericState;
        }

        #region IDssSerializable

        /// <summary>
        /// Copy To Analog Input sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXRoverDriveState typedTarget = target as VEXRoverDriveState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.DistanceBetweenWheels = this.DistanceBetweenWheels;
            typedTarget.IsEnabled = this.IsEnabled;
            typedTarget.leftEncoder = this.leftEncoder;
            typedTarget.leftFront = this.leftFront;
            typedTarget.leftMiddle = this.leftMiddle;
            typedTarget.leftRear = this.leftRear;
            typedTarget.rightEncoder = this.rightEncoder;
            typedTarget.rightFront = this.rightFront;
            typedTarget.rightMiddle = this.rightMiddle;
            typedTarget.rightRear = this.rightRear;
            typedTarget.TimeStamp = this.TimeStamp;
        }

        /// <summary>
        /// Clone the VEX Limit Switch State
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            VEXRoverDriveState clone = new VEXRoverDriveState();
            clone.DistanceBetweenWheels = this.DistanceBetweenWheels;
            clone.IsEnabled = this.IsEnabled;
            clone.leftEncoder = this.leftEncoder;
            clone.leftFront = this.leftFront;
            clone.leftMiddle = this.leftMiddle;
            clone.leftRear = this.leftRear;
            clone.rightEncoder = this.rightEncoder;
            clone.rightFront = this.rightFront;
            clone.rightMiddle = this.rightMiddle;
            clone.rightRear = this.rightRear;
            clone.TimeStamp = this.TimeStamp;

            clone.runtimeStatistics = this.runtimeStatistics;

            return clone;
        }

        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(this.DistanceBetweenWheels);
            writer.Write(this.IsEnabled);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            DistanceBetweenWheels = reader.ReadDouble();
            IsEnabled = reader.ReadBoolean();
            TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);

            return this;
        }
        #endregion
    }

    /// <summary>
    /// VEX Rover Drive Runtime Statistics
    /// </summary>
    [DataContract, Description("Provides Runtime statistics for the VEX Rover Drive.")]
    public class RuntimeStatistics : ICloneable
    {
        /// <summary>
        /// Indicates the Left motor power; range is -1.0 to 1.0.
        /// </summary>
        [DataMember, Description("Indicates the current power of the Left Motor. \n(range is -1.0 to 1.0)")]
        public double LeftPowerCurrent;

        /// <summary>
        /// Indicates the Left motor target power; range is -1.0 to 1.0.
        /// </summary>
        [DataMember, Description("Indicates the desired Target Power of the Left motor. \n(range is -1.0 to 1.0)")]
        public double LeftPowerTarget;

        /// <summary>
        /// Left Motor Encoder.  Current reading in tics.
        /// </summary>
        [DataMember, Description("Indicates the current reading of the Left Motor Encoder in tics.")]
        public long LeftEncoderCurrent;
        public long LeftTicksSinceReset;

        /// <summary>
        /// Left Motor Encoder Target.  Desired target of the Left encoder in tics.
        /// </summary>
        [DataMember, Description("Indicates the Desired Target of the Left Motor Encoder in tics.")]
        public long LeftEncoderTarget;

        /// <summary>
        /// Indicates the Right motor power; range is -1.0 to 1.0.
        /// </summary>
        [DataMember, Description("Indicates the current power of the Right Motor. \n(range is -1.0 to 1.0)")]
        public double RightPowerCurrent;

        /// <summary>
        /// Indicates the Right motor target power; range is -1.0 to 1.0.
        /// </summary>
        [DataMember, Description("Indicates the desired Target Power of the Right motor. \n(range is -1.0 to 1.0)")]
        public double RightPowerTarget;

        /// <summary>
        /// Right Motor Encoder.  Current reading in tics.
        /// </summary>
        [DataMember, Description("Indicates the current reading of the Right Motor Encoder in tics.")]
        public long RightEncoderCurrent;
        public long RightTicksSinceReset;

        /// <summary>
        /// Right Motor Encoder Target.  Desired target of the Right encoder in tics.
        /// </summary>
        [DataMember, Description("Indicates the Desired Target of the Right Motor Encoder in tics.")]
        public long RightEncoderTarget;

        /// <summary>
        /// Right Encoder TimeStamp.
        /// </summary>
        [DataMember, Description("Indicates the time of the Right Encoder reading.")]
        public DateTime TimeStamp;

        #region ICloneable Members

        /// <summary>
        /// Clone RuntimeStatistics
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            RuntimeStatistics clone = new RuntimeStatistics();
            clone.LeftPowerCurrent = this.LeftPowerCurrent;
            clone.RightPowerCurrent = this.RightPowerCurrent;
            clone.LeftEncoderCurrent = this.LeftEncoderCurrent;
            clone.RightEncoderCurrent = this.RightEncoderCurrent;
            clone.TimeStamp = this.TimeStamp;
            clone.LeftEncoderTarget = this.LeftEncoderTarget;
            clone.RightEncoderTarget = this.RightEncoderTarget;
            clone.LeftPowerTarget = this.LeftPowerTarget;
            clone.RightPowerTarget = this.RightPowerTarget;
            return clone;
        }

        #endregion
    }


    /// <summary>
    /// VEX Wheel Configuration
    /// </summary>
    [DataContract, Description("Specifies the VEX Wheel Configuration.")]
    public class WheelConfiguration : ICloneable
    {
        #region Constructors
        /// <summary>
        /// VEX Wheel Configuration
        /// </summary>
        public WheelConfiguration()
        {
        }

        /// <summary>
        /// VEX Wheel Configuration
        /// </summary>
        /// <param name="motorPort"></param>
        /// <param name="reversePolarity"></param>
        /// <param name="wheelDiameter"></param>
        public WheelConfiguration(vex.MotorPort motorPort, bool reversePolarity, double wheelDiameter)
        {
            this.MotorPort = motorPort;
            this.ReversePolarity = reversePolarity;
            this.WheelDiameter = wheelDiameter;
        }
        #endregion

        /// <summary>
        /// Motor Port Configuration
        /// </summary>
        [DataMember, Description("Specifies the Motor Port Configuration.")]
        [DataMemberConstructor(Order = 1)]
        public vex.MotorPort MotorPort;

        /// <summary>
        /// Reverse Motor Polarity
        /// </summary>
        [DataMember, Description("Indicates the direction (polarity) of the motor.\n(Enabling this option (true) reverses the motor.)")]
        [DataMemberConstructor(Order = 2)]
        public bool ReversePolarity;

        /// <summary>
        /// Diameter of the wheel (meters)
        /// </summary>
        [DataMember, Description("Specifies the diameter of the wheel in meters \n(Example 5.2cm = 0.052)")]
        [DataMemberConstructor(Order = 3)]
        public double WheelDiameter;

        #region ICloneable Members

        /// <summary>
        /// Deep Clone WheelConfiguration
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            WheelConfiguration clone = new WheelConfiguration(this.MotorPort, this.ReversePolarity, this.WheelDiameter);
            return clone;
        }

        #endregion
    }

    /// <summary>
    /// VEX specific drive request which stops after the specified motor rotation degrees.
    /// </summary>
    [DataContract, Description("Specifies a VEX specific drive request which stops after the specified motor rotation degrees.")]
    public class SetDriveRequest
    {
        /// <summary>
        /// Left Motor Power (-1.0 - 1.0)
        /// </summary>
        [DataMember, Description("Specifies the Left Motor Power (-1.0 - 1.0)")]
        [DataMemberConstructor(Order = 1)]
        public double LeftPower;

        /// <summary>
        /// Right Motor Power (-1.0 - 1.0)
        /// </summary>
        [DataMember, Description("Specifies the Right Motor Power (-1.0 - 1.0)")]
        [DataMemberConstructor(Order = 2)]
        public double RightPower;

        /// <summary>
        /// Stop the Left Motor after it has rotated the specified degrees (0-continuous).
        /// </summary>
        [DataMember, Description("Stops the Left Motor after it has rotated the specified degrees (0-continuous).")]
        [DataMemberConstructor(Order = 3)]
        public long LeftStopAtRotationDegrees;

        /// <summary>
        /// Stop the Right Motor after it has rotated the specified degrees (0-continuous).
        /// </summary>
        [DataMember, Description("Stops the Right Motor after it has rotated the specified degrees (0-continuous).")]
        public long RightStopAtRotationDegrees;

        /// <summary>
        /// This request orignated from a generic operation
        /// </summary>
        /// 
        [DataMember, Description("Request originated from a generic operation.")]
        [DataMemberConstructor(Order = -1)]
        public bool isGenericOperation;

        /// <summary>
        /// DriveDistance stage
        /// </summary>
        /// 
        [DataMember, Description("Drive distance stage.")]
        [DataMemberConstructor(Order = -1)]
        public drive.DriveStage DriveDistanceStage;

        /// <summary>
        /// RotageDegrees stage
        /// </summary>
        /// 
        [DataMember, Description("Rotate degrees stage.")]
        [DataMemberConstructor(Order = -1)]
        public drive.DriveStage RotateDegreesStage;


        /// <summary>
        /// Drive request;
        /// </summary>
        [DataMember, Description("Drive request.")]
        [DataMemberConstructor(Order = -1)]
        public drive.DriveRequestOperation DriveRequestOperation;
    }

    /// <summary>
    /// Reset the Motor Encoder Position
    /// </summary>
    [DataContract, Description("Resets the Motor Encoder Position.")]
    public class ResetMotorPositionRequest
    {
        /// <summary>
        /// Relative to last position (true) or Absolute (false)
        /// </summary>
        [DataMember, Description("Specifies that the motor is to be reset Relative to last position (true).")]
        public bool Relative;
    }

    #region Operation Types

    /// <summary>
    /// Get the VEX Rover Drive State
    /// </summary>
    [Description("Gets the current state of the VEX Rover Drive.")]
    public class Get : Get<GetRequestType, PortSet<VEXRoverDriveState, Fault>>
    {
    }


    /// <summary>
    /// Run Motor
    /// </summary>
    [XmlTypeAttribute(IncludeInSchema = false)]
    [Description("Runs the VEX Rover Drive at the specified left and right motor power.\nOptionally stop afer specified degrees.")]
    public class DriveDistance : Update<SetDriveRequest, PortSet<DefaultUpdateResponseType, Fault>>
    {
        /// <summary>
        /// Run Motor
        /// </summary>
        public DriveDistance()
        {
        }
        /// <summary>
        /// Run Motor
        /// </summary>
        public DriveDistance(SetDriveRequest body)
            :
                base(body)
        {
        }
        /// <summary>
        /// Run Motor
        /// </summary>
        public DriveDistance(SetDriveRequest body, PortSet<DefaultUpdateResponseType, Fault> responsePort)
            :
                base(body, responsePort)
        {
        }
    }

    /// <summary>
    /// Subscribe to VEX notifications
    /// </summary>
    [Description("Subscribes to VEX notifications.")]
    public class Subscribe : Subscribe<SubscribeRequestType, PortSet<SubscribeResponseType, Fault>>
    {
    }


    #endregion

    /// <summary>
    /// VEX Rover Drive Operations Port
    /// </summary>
    [ServicePort]
    [XmlTypeAttribute(IncludeInSchema = false)]
    public class DriveOperations : PortSet
    {
        /// <summary>
        /// VEX Rover Drive Operations Port
        /// </summary>
        public DriveOperations()
            : base(
                typeof(DsspDefaultLookup),
                typeof(DsspDefaultDrop),
                typeof(Get),
                typeof(dssphttp.HttpGet),
                typeof(dssphttp.HttpPost),
                typeof(drive.AllStop),
                typeof(DriveDistance),
                typeof(drive.ResetEncoders),
                typeof(Subscribe))
        { }

        #region Implicit Operators
        /// <summary>
        /// Implicit Operator for Port of DsspDefaultLookup
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<DsspDefaultLookup>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<DsspDefaultLookup>)portSet[typeof(DsspDefaultLookup)];
        }
        /// <summary>
        /// Implicit Operator for Port of DsspDefaultDrop
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<DsspDefaultDrop>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<DsspDefaultDrop>)portSet[typeof(DsspDefaultDrop)];
        }
        /// <summary>
        /// Implicit Operator for Port of Get
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<Get>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<Get>)portSet[typeof(Get)];
        }
        /// <summary>
        /// Implicit Operator for Port of Microsoft.Dss.Core.DsspHttp.HttpGet
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<Microsoft.Dss.Core.DsspHttp.HttpGet>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<Microsoft.Dss.Core.DsspHttp.HttpGet>)portSet[typeof(Microsoft.Dss.Core.DsspHttp.HttpGet)];
        }
        /// <summary>
        /// Implicit Operator for Port of Microsoft.Dss.Core.DsspHttp.HttpPost
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<Microsoft.Dss.Core.DsspHttp.HttpPost>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<Microsoft.Dss.Core.DsspHttp.HttpPost>)portSet[typeof(Microsoft.Dss.Core.DsspHttp.HttpPost)];
        }
        /// <summary>
        /// Implicit Operator for Port of AllStop
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<drive.AllStop>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<drive.AllStop>)portSet[typeof(drive.AllStop)];
        }
        /// <summary>
        /// Implicit Operator for Port of DriveDistance
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<DriveDistance>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<DriveDistance>)portSet[typeof(DriveDistance)];
        }
        /// <summary>
        /// Implicit Operator for Port of Subscribe
        /// </summary>
        /// <param name="portSet"></param>
        /// <returns></returns>
        public static implicit operator Port<Subscribe>(DriveOperations portSet)
        {
            if (portSet == null) return null;
            return (Port<Subscribe>)portSet[typeof(Subscribe)];
        }
        #endregion

        #region Post
        /// <summary>
        /// Post(DsspDefaultLookup)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(DsspDefaultLookup item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(DsspDefaultDrop)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(DsspDefaultDrop item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(Get)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(Get item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(dssphttp.HttpGet)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(dssphttp.HttpGet item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(dssphttp.HttpPost)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(dssphttp.HttpPost item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(motor.AllStop)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(drive.AllStop item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(DriveDistance)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(DriveDistance item) { base.PostUnknownType(item); }
        /// <summary>
        /// Post(Subscribe)
        /// </summary>
        /// <param name="item"></param>
        /// <returns></returns>
        public void Post(Subscribe item) { base.PostUnknownType(item); }
        #endregion
    }
}
